#include "agv_util/point_util.hpp"
#include <iostream>
#include <vector>
// 自定义
#include "nav_msg_pkg/msg/waypoint.hpp"
void load_waypoint_test()
{
    auto waypoints = agv_util::load_waypoint_neighbors();
    std::cout << "路径点个数: " << waypoints.size() << std::endl;
    for (auto &waypoint : waypoints)
    {
        std::cout << "Point: " << waypoint.name << std::endl;
        std::cout << "Neighbors: ";
        for (const auto &neighbor : waypoint.neighbors)
        {
            std::cout << neighbor << " ";
        }
        std::cout << std::endl;
    }
}
int main()
{
    load_waypoint_test();
    return 0;
}